#include <iostream>
#include <fstream>
#include "Kalman.h"

using namespace std;
using namespace Eigen;

bool GetLidarData(double &x,double &y, double &timeStamp)
{
    static ifstream ifs("data.txt");
    if(!ifs.is_open())
    {
        cout << "data file open failed!\n";
        return false;
    }
    ifs >> x >> y >> timeStamp;
    if(ifs.eof())
    {
        cout << "reach the end of file!\n";
        return false;
    }
    return true;
}

int main(int argc, char *argv[])
{
    double m_x = 0., m_y = 0.;
    double last_timestamp = 0., now_timestamp = 0.;
    KalmanFilter kf;
    while (GetLidarData(m_x,m_y,now_timestamp)) {
        if(!kf.isInitialized()){
            last_timestamp = now_timestamp;

            Eigen::VectorXd x_in(4,1);
            x_in << m_x, m_y, 0.0, 0.0;
            kf.Initialization(x_in);

            Eigen::MatrixXd P_in(4,4);
            P_in << 1.0, 0.0, 0.0, 0.0,
                    0.0, 1.0, 0.0, 0.0,
                    0.0, 0.0, 100.,0.0,
                    0.0, 0.0, 0.0, 100.;
            kf.setP(P_in);

            Eigen::MatrixXd Q_in(4,4);
            Q_in << 1.0, 0.0, 0.0, 0.0,
                    0.0, 1.0, 0.0, 0.0,
                    0.0, 0.0, 1.0, 0.0,
                    0.0, 0.0, 0.0, 1.0;
            kf.setQ(Q_in);

            Eigen::MatrixXd R_in(2,2);
            R_in << 1.0, 0.0,
                    0.0, 1.0;
            kf.setR(R_in);

            Eigen::MatrixXd H_in(2,4);
            H_in << 1.0, 0.0, 0.0, 0.0,
                    0.0, 1.0, 0.0, 0.0,
            kf.setH(H_in);
            continue;
        }
        double delta_t = now_timestamp - last_timestamp;
        last_timestamp = now_timestamp;
        Eigen::MatrixXd F_in(4,4);
        F_in << 1.0, 0.0, delta_t, 0.0,
                0.0, 1.0, 0.0, delta_t,
                0.0, 0.0, 1.0, 0.0,
                0.0, 0.0, 0.0, 1.0;
        kf.setF(F_in);
        kf.Prediction();
        Eigen::VectorXd z(2,1);
        kf.MeasurementUpdate(z);
        Eigen::VectorXd x_out(4,1);
        kf.getState(x_out);
        std::cout << "kalman output state: x = " << x_out(0)
                  << ", y = " << x_out(1) << std::endl;
    }

    return 0;
}
